﻿//This script is for getting and analysing the datas on a horizontal line  on a certain point  of  body, like waist
//We will draw it on a graph.

//This script is part 0, which is for getting the 1D data each frame, filling empty gaps and analyse them to get
//the boundary of the body. Cull out the range of arms.

//7  points  for  Gussian blur
//0.00000067	0.00002292	0.00019117	0.00038771	0.00019117	0.00002292	0.00000067
//	0.00002292	0.00078633	0.00655965	0.01330373	0.00655965	0.00078633	0.00002292
//		0.00019117	0.00655965	0.05472157	0.11098164	0.05472157	0.00655965	0.00019117
//		0.00038771	0.01330373	0.11098164	0.22508352	0.11098164	0.01330373	0.00038771
//		0.00019117	0.00655965	0.05472157	0.11098164	0.05472157	0.00655965	0.00019117
//		0.00002292	0.00078633	0.00655965	0.01330373	0.00655965	0.00078633	0.00002292
//		0.00000067	0.00002292	0.00019117	0.00038771	0.00019117	0.00002292	0.00000067



//5 points for 1D Gaussian Blur   from  7   26   41  26 7
//0.065   0.243    0.383    0.243   0.065




using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;




public partial class FrontMeaMgr : MonoBehaviour {

	public bool enableDrawing=true;
	float mNormalDepth=0;
	
	int depthW;
	int depthH;

	float[]  Depth1DArray;
	float[]  Depth1DArrayBack;
	
	float[]  normalizedDepthArray; // we will use guassian and canny on this float later
	float[]  normalizedDepthArrayBack;  //We will use guassian on the previous one , so we need a buffer.

	float[]  sobelArray;
	float[]  sobelDiff;
		
	float[]  gaussian1D7Pts;
	float[]  gaussian1D5Pts;


	int baseDepthRulerDist=2500;  //in mm. We use this as 

	ushort FarNearDist;
	float CenterOutDist=300f;  //The value for determining the difference between center body part and the out boundary of arm
	
	public GameObject chartObj0;
	Texture2D depthAnalysisTex;
	Color32[] depthAnaColorArr;
	public GameObject chartObj1;
	Texture2D SobelTex;
	Color32[] SobelClrArr;
	
	//char for showing local depth datas;
	int chartW=320;
	int chartH=200;
	
	int tmpChartLen=0;
	int Imgcount=0;
	float localIota=0.01f;



	public int boundaryLeftIdx=0;
	public int boundaryRightIdx=0;
	public int boundaryY=0;
	public  ArrayList boundaryYIdxArr=new ArrayList();
	public  ArrayList boundaryLeftIdxArr=new ArrayList();

	//Sharp idx is the idx begin with the outside countour boundary to the arm boundary, usually it is small
	public int sharpBeginIdx=0;
	public int sharpEndIdx=1;
	public  ArrayList SharpBeginIdxArr=new ArrayList();
	

	//armBodyGapLeftIdx is the idx begin with the sharp idx to the pit between arm and body. It is the boundary idx of body boundary calculated since sharp idx.
	//The X idx of the pit between arm and body,aka,the body boundary,  on a depth map,  is  boundaryLeftIdx+ sharpBeginIdx+  armBodyGapLeftIdx
    public	int armBodyGapLeftIdx=0;
    public	int armBodyGapRightIdx=0;
	public ArrayList armBodyGapLeftIdxArr=new ArrayList();
	public ArrayList armBodyGapRightIdxArr=new ArrayList();
	
	 
	//Since 2017 06 29    This following paras are for getting the results of measuring the front chord length of a body.
	public float frontLen0=0f;   //The front arc len.
	public float frontLen1=0f;   //The front chord len.
	public float frontSideChordThick=0f;

	// Use this for initialization
	void Start () {
	
		normalizedDepthArray=new float[chartW];
		normalizedDepthArrayBack=new float[chartW];
		Depth1DArray=new float[chartW];
		Depth1DArrayBack=new float[chartW];


		ZeroAllArrs();


	
		sobelArray=new float[chartW];
		sobelDiff=new float[chartW];
		
		
//		gussian1D7Pts=new float[]
//		{0.00038771f,
//			0.01330373f,
//			0.11098164f,
//			0.22508352f,
//			0.11098164f,
//			0.01330373f,
//			0.00038771f};

		//gaussian1D5Pts=new float[]{0.065f,0.243f,0.383f,0.243f,0.065f};
		gaussian1D5Pts=new float[]{0.2f,0.2f,0.2f,0.2f,0.2f};
		
		gaussian1D7Pts=new float[]
				{   
			        0.142f,
					0.142f,
					0.142f,
					0.142f,
					0.142f,
					0.142f,
					0.142f
		         };


		depthW = 640;
		depthH = 480;
		
		depthAnalysisTex=new Texture2D(chartW,chartH);
		depthAnaColorArr=new Color32[chartW*chartH];

		if(chartObj0!=null)
		{
		 chartObj0.GetComponent<Renderer>().material.mainTexture=depthAnalysisTex;
		}
//		SobelTex=new Texture2D(chartW,chartH);
//		SobelClrArr=new Color32[chartW*chartH];
//		chartObj1.GetComponent<Renderer>().material.mainTexture=SobelTex;

		if(!isDevCheckedThisTime)
		{
	     	CheckString();
		}
	}


	void ZeroAllArrs()
	{
		for(int i=0;i<chartW;i++)
		{
			normalizedDepthArray[i]=0.0f;
			normalizedDepthArrayBack[i]=0.0f;
			Depth1DArray[i]=0.0f;
			Depth1DArrayBack[i]=0.0f;
		}
	}

	public void ClearSampleDatas()
	{
		boundaryLeftIdx=0;
		boundaryRightIdx=0;
		
		sharpBeginIdx=0;
		sharpEndIdx=1;
		
		armBodyGapLeftIdx=0;
		armBodyGapRightIdx=0;
		ZeroAllArrs();
	}


	// Update is called once per frame
	void Update () 
	{
		if(Input.GetKey(KeyCode.LeftControl)&&(Input.GetKey(KeyCode.LeftAlt)))
		{
			if(Input.GetKeyDown(KeyCode.Y))
			{
				enableFunction=!enableFunction;
				
			}
		}
	}



	public void DrawGraph()
	{
		depthAnalysisTex.SetPixels32(depthAnaColorArr);
		depthAnalysisTex.Apply();
	}



   public	void DrawDepthAnalysisArr(
		                            //                     float[]  normalizedDepthArray,   
	                                  // int HoriLinePixsCounts
		)
	{
		//Draw depth analyssi lines on a chart

		int arrlen=normalizedDepthArray.Length;
		if(arrlen>chartW){arrlen=chartW;   }



		for(int i=0;i<arrlen;i++)
		{
//			if((i%25)==0)
//			{
//			Debug.Log("normalizedDepthArray depth val  "+i.ToString()+"   "+normalizedDepthArray[i].ToString()+"  "+Depth1DArray[i].ToString());
//			}
			int chartLen=(int)(normalizedDepthArray[i]*(float)chartH);
			if(chartLen>=chartH)chartLen=chartH-1;
			
			tmpChartLen=chartLen;

			if(( i==armBodyGapLeftIdx )||( i==armBodyGapRightIdx ) )
			{
				//Draw the left and right body boundary. Which is the arm pit between arm and body.
				for(int k=0;k<chartLen;k++)
				{
					depthAnaColorArr[k*chartW+i].r=255;
					depthAnaColorArr[k*chartW+i].g=0;
					depthAnaColorArr[k*chartW+i].b=0;
					depthAnaColorArr[k*chartW+i].a=255;				
				}
			}
			else
			{
				for(int k=0;k<chartLen;k++)
				{
					depthAnaColorArr[k*chartW+i].r=0;
					depthAnaColorArr[k*chartW+i].g=255;
					depthAnaColorArr[k*chartW+i].b=0;
					depthAnaColorArr[k*chartW+i].a=255;				
				}
			}
		}
	}


	public  void DrawFinalDataDepthVal()
	{
		//Draw depth analyssi lines on a chart
		
		int arrlen=FinalBodyDepth1DArray.Length;
		if(arrlen>chartW){arrlen=chartW;   }
		
		
		
		for(int i=0;i<arrlen;i++)
		{
			//			if((i%25)==0)
			//			{
			//			Debug.Log("normalizedDepthArray depth val  "+i.ToString()+"   "+normalizedDepthArray[i].ToString()+"  "+Depth1DArray[i].ToString());
			//			}
			int chartLen=(int)(FinalBodyDepth1DArray[i]/1000f*(float)chartH);
			if(chartLen>=chartH)chartLen=chartH-1;
			
			tmpChartLen=chartLen;
			

				//Draw the left and right body boundary. Which is the arm pit between arm and body.
				for(int k=0;k<chartLen;k++)
				{
					depthAnaColorArr[k*chartW+i].r=255;
					depthAnaColorArr[k*chartW+i].g=0;
					depthAnaColorArr[k*chartW+i].b=0;
					depthAnaColorArr[k*chartW+i].a=255;				
				}

		}
	}


	public void ClearGraph()
	{
		for(int i=0;i<depthAnaColorArr.Length;i++)
		{
			depthAnaColorArr[i].r=0;
			depthAnaColorArr[i].g=0;
			depthAnaColorArr[i].b=0;
			depthAnaColorArr[i].a=255;	

		}
	}


	public  void FillRowNormaliezedDepthDataAndFillZeroGap(ushort[] depthUserData, int pixPosXMin, int pixPosXMax,int pixPosY,int depthW, int depthH)
	{
		FillRowNormaliezedDepthDataToArr(depthUserData,  pixPosXMin,  pixPosXMax, pixPosY, depthW,  depthH);
		FillEmptyDepthArr();
		RearrangeData();
		LocateBodyBoundary();
		StoreBody1DAnalysedData();
	}

	//Store 1D data into the Arr
	void FillRowNormaliezedDepthDataToArr(ushort[] depthUserData, int pixPosXMin, int pixPosXMax,int pixPosY,int depthW, int depthH)
	{
		if(EnableFunction)
		{
		boundaryLeftIdx=pixPosXMin;
		boundaryRightIdx=pixPosXMax;
		boundaryY=pixPosY;

		boundaryYIdxArr.Add(pixPosY);
		boundaryLeftIdxArr.Add(pixPosXMin);

		int len=pixPosXMax-pixPosXMin;
		if(len>chartW)
		{
			len=chartW;
		}

		for(int i=pixPosXMin,j=0;i<pixPosXMin+len;i++,j++)
		{ 
			int tmpValue=depthUserData[depthW*pixPosY+ i]>>3;
			//Debug.Log( i.ToString()+"  "+ tmpValue.ToString());

			if((tmpValue>=baseDepthRulerDist)||(tmpValue<1300))
			{
				tmpValue=5000;  //Make this depth value bigger, so we the height will be 0 on the chart.
			}

			float DepthSampleVal=(float)(baseDepthRulerDist-tmpValue);
			if(DepthSampleVal<0f)
			{
				DepthSampleVal=0f;
			}

			Depth1DArray[j]=DepthSampleVal;
			Depth1DArrayBack[j]=DepthSampleVal;

			mNormalDepth= ((float)(baseDepthRulerDist-tmpValue))/((float)(1200));
			if(mNormalDepth>1f)
			{
				mNormalDepth=1f;
			}	

			if(mNormalDepth<0)
			{
				mNormalDepth=0f;
			}	
			
			normalizedDepthArray[j]=mNormalDepth;
			//normalizedDepthArrayBack[j]=mNormalDepth;
		}
		}
	}


	//This func fill the empty gap of  normalizedDepthArray, namely the 1D sampling of certain body part , like waist.
	//In this part, we use a "Mirror" strategy. The missing part will be filled by the mirrored sampling, 
	//since body are left/right mirrored.
	//The we use a gradient way to fill the left empty gap remains. We fill the gap by linear from left unzero to right unzero.
	//Attention  The current filling and smooth data stored in normalizedDepthArrayBack
	#region func for filling empty gap and smooth samplijng.  Attention  The current filling and smooth data stored in normalizedDepthArrayBack
	void FillEmptyDepthArr()
	{
		int DepthArrLen=Depth1DArray.Length;
		int beginIdx=0;  //To store the first no-zero 1D sampling of the body, as one limit of the body.
		int endIdx=0;  //To store the last no-zero 1D sampling of the body, as another limit of the body.
		int middleIdx=0;

		//locate one end of data;
		for(int i=0;i<DepthArrLen;i++)
		{
			if(Depth1DArray[i]>localIota)
			{
				beginIdx=i;
				break;
			}
		}

		//locate the other end of data
		for(int i=DepthArrLen-1;i>=0;i--)
		{
			if(Depth1DArray[i]>localIota)
			{
				endIdx=i;
				break;
			}
		}

		if(((endIdx-beginIdx)>5)&&(beginIdx>=0))
		{
			middleIdx= (beginIdx+endIdx)/2;



#region	 2nd loop , fill zero gap remains with gradient value.
			for(int i=beginIdx;i<endIdx;i++)
			{			
				if((normalizedDepthArray[i]<=localIota)&&(i>0))
				{
					bool foundAnotherEnd=false;

					//find out how wide the zero gap is.  and fill it with gradient method.
					for(int j=i+1;j<endIdx;j++)
					{
						if(normalizedDepthArray[j]>localIota)
						{

							foundAnotherEnd=true;
							int gapnumber=j-i;


//method 0 use gradiant value to fill the zero gap between 2 no-zero sampling value.
//							float addValue= (normalizedDepthArray[j]-normalizedDepthArray[i-1])/((float)(gapnumber));
//							float addValue2= (Depth1DArray[j]-Depth1DArray[i-1])/((float)(gapnumber));
//							
//							for(int k=i;k<j;k++)
//							{
//								normalizedDepthArray[k]=normalizedDepthArray[k-1]+addValue;
//								Depth1DArray[k]=Depth1DArray[k-1]+addValue2;
//							}


//method 1 use the smaller value of  [i-1]   and   [j]    to fill the zero  gap between them
							float smallerValue=normalizedDepthArray[i-1];
							if(normalizedDepthArray[i-1]>normalizedDepthArray[j])
							{
								smallerValue=normalizedDepthArray[j];
							}

							float smallerValue1=Depth1DArray[i-1];
							if(Depth1DArray[i-1]>Depth1DArray[j])
							{
								smallerValue1=Depth1DArray[j];
							}

							for(int k=i;k<j;k++)
							{
								normalizedDepthArray[k]=smallerValue;
								Depth1DArray[k]=smallerValue1;
							}


							i=j;
							break;
						}
					}
				}


			}
#endregion

		
#region  3rd loop, use 5 or 7 points  Gaussian or Average Filter to smooth the values.  Attention, Since 2017 07 we use average filter. But the array name is still gussian
	
//			if( endIdx-beginIdx>6 )
//			{
//				for(int i=beginIdx+2;i<endIdx-2;i++)
//				{
//
//					normalizedDepthArray[i]=normalizedDepthArrayBack[i-2]*gaussian1D5Pts[0]+
//										    normalizedDepthArrayBack[i-1]*gaussian1D5Pts[1]+
//											normalizedDepthArrayBack[i]*gaussian1D5Pts[2]+
//											normalizedDepthArrayBack[i+1]*gaussian1D5Pts[3]+
//											normalizedDepthArrayBack[i+2]*gaussian1D5Pts[4];
//				}
//			}

			//OR  We can use 7 points.

			//Since 2017 07 19, we use 4 points average filter. the multi value is 0.25
		     	if( endIdx-beginIdx>8 )
				{
				// before 2017  07, use 7 points for smooth.

//					for(int i=beginIdx+3;i<endIdx-3;i++)
//					{	
//						normalizedDepthArrayBack[i]=normalizedDepthArray[i-3]*gaussian1D7Pts[0]+
//												    normalizedDepthArray[i-2]*gaussian1D7Pts[1]+
//													normalizedDepthArray[i-1]*gaussian1D7Pts[2]+
//													normalizedDepthArray[i]*gaussian1D7Pts[3]+
//													normalizedDepthArray[i+1]*gaussian1D7Pts[4]+
//													normalizedDepthArray[i+2]*gaussian1D7Pts[5]+
//													normalizedDepthArray[i+3]*gaussian1D7Pts[6];
//
//								Depth1DArrayBack[i]=Depth1DArray[i-3]*gaussian1D7Pts[0]+
//												    Depth1DArray[i-2]*gaussian1D7Pts[1]+
//													Depth1DArray[i-1]*gaussian1D7Pts[2]+
//													Depth1DArray[i]*gaussian1D7Pts[3]+
//													Depth1DArray[i+1]*gaussian1D7Pts[4]+
//													Depth1DArray[i+2]*gaussian1D7Pts[5]+
//													Depth1DArray[i+3]*gaussian1D7Pts[6];
//				    }


				//Since  2017 07 17,  use 4 points for filter .
				for(int i=beginIdx+2;i<endIdx-2;i++)
										{	
											normalizedDepthArrayBack[i]=(normalizedDepthArray[i-2]+
						                                                normalizedDepthArray[i-1]+
							                                            normalizedDepthArray[i]+
							                                            normalizedDepthArray[i+1])*0.25f;
																		
					
													Depth1DArrayBack[i]=(Depth1DArray[i-2]+
																		Depth1DArray[i-1]+
																		Depth1DArray[i]+
							                                            Depth1DArray[i+1])*0.25f;
									    }


				}

#endregion
		}
	}

#endregion


	//ReArrange the range of left arm to right arm. Sample out some of boundary Redundant data. Keep only absolute left arm to right arm range.
	//From   normalizedDepthArrayBack    to    normalizedDepthArray.
	#region for get very strict left to right arm to arm boundary.
	void RearrangeData()
	{
		int DepthArrLen=normalizedDepthArrayBack.Length;
		int beginIdx=0;  //To store the first no-zero 1D sampling of the body, as one limit of the body.
		int endIdx=0;  //To store the last no-zero 1D sampling of the body, as another limit of the body.
		int middleIdx=0;
		
		//locate one end of data;
		for(int i=0;i<DepthArrLen;i++)
		{
			if(normalizedDepthArrayBack[i]>localIota)
			{
				beginIdx=i;
				break;
			}
		}
		
		
		
		//locate the other end of data
		for(int i=DepthArrLen-1;i>=0;i--)
		{
			if(normalizedDepthArrayBack[i]>localIota)
			{
				endIdx=i;
				break;
			}
		}

		middleIdx=(beginIdx+endIdx)/2;

		//Find the ave depth value of the center part of the torso. We use this to find out the beginning arm boundary.  
		//We set this by supposing that the difference of beginning arm boundary and ave center depth value is less than 30 cm.
		if(((endIdx-beginIdx)>5)&&(beginIdx>=0))
		{

			int centerBeginIdx=beginIdx+(int)((float)(endIdx-beginIdx)*0.43f);
			int centerEndIdx=beginIdx+(int)((float)(endIdx-beginIdx)*0.57f);


			if( centerEndIdx>centerBeginIdx  )
			{
				float sumofcenterPart=0f;

			    //Clear the array . We will fill it with averaged BackArray.
				for(int i=0;i<normalizedDepthArray.Length;i++)
				{
					normalizedDepthArray[i]=0f;
				    Depth1DArray[i]=0f;
				}

				for(int i=centerBeginIdx;i<centerEndIdx;i++)
				{
					sumofcenterPart+=Depth1DArrayBack[i];
				}
				float aveOfCenterPart=sumofcenterPart/( centerEndIdx- centerBeginIdx);
                   

				//Debug.Log("Center Average depth   "+  aveOfCenterPart.ToString() );

				 sharpBeginIdx=0;
				 sharpEndIdx=1;

				//Get the shart beging and end idx;
				for(int i=0;i<middleIdx;i++)
				{
					if( Math.Abs(Depth1DArrayBack[i]-aveOfCenterPart)<CenterOutDist)
					{
						sharpBeginIdx=i;
						break;
					}
				}

				for(int i=endIdx;i>middleIdx;i--)
				{
					if( Math.Abs(Depth1DArrayBack[i]-aveOfCenterPart)<CenterOutDist)
					{
						sharpEndIdx=i;
						break;
					}					
				}


//				Debug.Log("SharpBeginIdx   "+ sharpBeginIdx.ToString());
//				Debug.Log("SharpEndIdx   "+ sharpEndIdx.ToString());
				

				SharpBeginIdxArr.Add(sharpBeginIdx);
				for(int i=sharpBeginIdx,j=0;i<sharpEndIdx;i++,j++)
				{
					normalizedDepthArray[j]=normalizedDepthArrayBack[i];
					Depth1DArray[j]=Depth1DArrayBack[i];
				}


			}
			else
			{
				Debug.LogError("Center Part range is zero");
			}
		}
	}

	#endregion


	//We use this func to get the boundary of body . Which is the pit between arm and body.  
	//We get the idx of the pit from analysing the data in Depth1DArray
	//Generally it is seemed as the lowest point in the position of about 4% to 30%  and  70% to 96%
	#region Get strict left right boundary of the body.  Which means we sample out the arm range.
	void LocateBodyBoundary()
	{
		if(EnableFunction)
		{
		int wholeLen=sharpEndIdx-sharpBeginIdx;
		int leftBeginIdx=0;
		int leftEndIdx=0;
		int rightBeginIdx=0;
		int rightEndIdx=0;
		int leftLowPointIdx=0;
		int rightLowPointIdx=0;		
		
		if(wholeLen>25)
		{
		    leftBeginIdx=(int)(0.06f*((float)(wholeLen)));
			leftEndIdx=(int)(0.2f*((float)(wholeLen)));
			rightBeginIdx=(int)(0.8f*((float)(wholeLen)));
			rightEndIdx=(int)(0.94f*((float)(wholeLen)));
			
			leftLowPointIdx=leftEndIdx;
			rightLowPointIdx=rightBeginIdx;

			//In this part we will find the pit point(lowest) of the datas
			for(int i=leftEndIdx;i>leftBeginIdx;i--)
			{
				if(Depth1DArray[i]<Depth1DArray[leftLowPointIdx])
				{
					leftLowPointIdx=i;
				}  
			} 

			//If the detected pit point is too close to the hand boundary,  we seem it as "not detecting the pit", means not lowest(pit) in the data
			//Then we use another way to detect the boundary between hands and body.
			//We locate the "Most down pit" point by campare data[i+2]+data[i-2]-2*(data[i]). The bigger the value is, the most down curved/dent it is.  
			if((leftLowPointIdx-leftBeginIdx)<2)
			{
				float diff0=0f;

				for(int i=leftEndIdx-3;i>leftBeginIdx+3;i--)
				{
					if(  (Depth1DArray[i+2]+Depth1DArray[i+3]+Depth1DArray[i-2]+Depth1DArray[i-3]-4*Depth1DArray[i])>diff0)
					{
						diff0=  (Depth1DArray[i+2]+Depth1DArray[i+3]+Depth1DArray[i-2]+Depth1DArray[i-3]-4*Depth1DArray[i]);
						leftLowPointIdx=i;
					}
				}
			}

			for(int i=rightBeginIdx;i<rightEndIdx;i++)
			{
				if(Depth1DArray[i]<Depth1DArray[rightLowPointIdx])
				{
					rightLowPointIdx=i;
				}
			}

			if((rightEndIdx-rightLowPointIdx)<2)
			{
				float diff1=0f;
				
				for(int i=rightBeginIdx+2;i<rightEndIdx-2;i++)
				{
					if(  (Depth1DArray[i+2]+Depth1DArray[i+3]+Depth1DArray[i-2]+Depth1DArray[i-3]-4*Depth1DArray[i])>diff1)
					{
						diff1=(Depth1DArray[i+2]+Depth1DArray[i+3]+Depth1DArray[i-2]+Depth1DArray[i-3]-4*Depth1DArray[i]);
						rightLowPointIdx=i;
					}
				}
			}

			armBodyGapLeftIdx=leftLowPointIdx;
			armBodyGapRightIdx=rightLowPointIdx;
			armBodyGapLeftIdxArr.Add(armBodyGapLeftIdx);
			armBodyGapRightIdxArr.Add(armBodyGapRightIdx);
		}
		else
		{
			//Debug.LogError("the whole len of body and 2 arm is left than 25 pixels. ");
		}
		
		}
	}

	#endregion




	void OnGUI()
	{

	}


}
